package com.zyj.utils.d3.base;

import com.zyj.utils.d3.entity.Point;
import com.zyj.utils.d3.entity.Vector;
import com.zyj.utils.maths.MathUtils;

/**
 * 线段工具方法
 */
public class SegmentUtils {
    /**
     * 计算线段AB、CD的最小距离
     * @param A
     * @param B
     * @param C
     * @param D
     * @return
     */
    public static double calculateSegmentsDist(Point A, Point B, Point C, Point D){
        if(PointUtils.towPointsIsSame(A, B)){
            //如果AB重合，则计算A到CD的距离
            return calculatePointToSegmentDist(A, C, D);
        }
        if(PointUtils.towPointsIsSame(C, D)){
            //如果CD重合，则计算C到AB的距离
            return calculatePointToSegmentDist(C, A, B);
        }
        /**
         * (1)如果AB、CD是平行线段，则求A到CD、B到CD的最小值，作为AB、CD的最小距离
         */
        if(SegmentUtils.isParallel(A, B, C, D)){
            return calculateParallelSegmentsDist(A, B, C, D);
        }

        /**
         * 计算AB、CD最小距离是否在AB、CD内
         */
        double dist = computeNotParalleSegmentsDist(A, B, C, D);
        return dist;
    }

    /**
     * 计算平行线段AB、CD最小距离
     * @param A
     * @param B
     * @param C
     * @param D
     * @return
     */
    public static double calculateParallelSegmentsDist(Point A, Point B, Point C, Point D) {
        double ACD = calculatePointToSegmentDist(A, C, D);
        double BCD = calculatePointToSegmentDist(B, C, D);
        return Math.min(ACD, BCD);
    }

    /**
     * 计算非平行线段AB、CD的最小距离；
     *  设A(x1, y1, z1)、B(x2, y2, z2)、C(x3, y3, z3)、D(x4, y4, z4)
     *  设P（X,Y,Z）在直线AB上，Q(U,V,W)在实现CD上
     *   {
     *      X = x1 + s(x2 - x1),
     *      Y = y1 + s(y2 - y1)
     *      Z = z1 + s(z2 - z1)
     *   },
     *   {
     *      U = x3 + t(x4 - x3),
     *      V = y3 + t(y4 - y3)
     *      W = z3 + t(z4 - z3)
     *   }
     *   当 0 <= s <= 1时，P在AB内， 当s < 0时，P在BA延长线，当s > 0时， P在AB延长线上。
     *   当 0 <= t <= 1时，Q在CD内， 当t < 0时，P在DC延长线，当t > 0时， P在CD延长线上。
     *
     *   PQ距离： PQ² = (X - U)² + (Y - W)² + (Z - V)²
     *   通过极限逼近，求得s、t的方程组：
     *   {
     *       fs1 * s + ft1 * t = r1;
     *       fs2 * s + ft2 * t = r2;
     *       其中：
     *          fs1 =  (x2 - x1)² + (y2 - y1)² + (z2 - z1)²；
     *          ft1 = -((x2 - x1) * (x4 - x3) + (y2 - y1) * (y4 - y3) + (z2 - z1) * (z4 - z3));
     *          r1 = (x1 - x2) * (x1 - x3) + (y1 - y2) * (y1 - y3) + (z1 - z2) * (z1 - z3);
     *          fs2 = ft1;
     *          ft2 = (x4 - x3)² + (y4 - y3)² + (z4 - z3)²；
     *          r2 = (x1 - x3) * (x4 - x3) + (y1 - y3) * (y4 - y3) + (z1 - z3) * (z4 - z3);
     *   }
     *
     * @param A
     * @param B
     * @param C
     * @param D
     * @return
     */
    public static double computeNotParalleSegmentsDist(Point A, Point B, Point C, Point D){
        double x1 = A.x, y1 = A.y, z1 = A.z;
        double x2 = B.x, y2 = B.y, z2 = B.z;
        double x3 = C.x, y3 = C.y, z3 = C.z;
        double x4 = D.x, y4 = D.y, z4 = D.z;

        double factor1_s = Math.pow((x2 - x1), 2) + Math.pow(y2 - y1, 2) + Math.pow(z2 - z1, 2);
        double factor1_t = -((x2 - x1) * (x4 - x3) + (y2 - y1) * (y4 - y3) + (z2 - z1) * (z4 - z3));
        double result1 = (x1 - x2) * (x1 - x3) + (y1 - y2) * (y1 - y3) + (z1 - z2) * (z1 - z3);

        double factor2_s = factor1_t;
        double factor2_t = Math.pow(x4 - x3, 2) + Math.pow(y4 - y3, 2) + Math.pow(z4 - z3, 2);
        double result2 = (x1 - x3) * (x4 - x3) + (y1 - y3) * (y4 - y3) + (z1 - z3) * (z4 - z3);
        double s = 0, t = 0;
        if(factor1_s == 0) {
            t = result1 / factor1_t;
            s = (result2 - factor2_t * t)/ factor2_s;
        }else if(factor1_t == 0) {
            s = result1 / factor1_s;
            t = (result2 - factor2_s * s) / factor2_t;
        }else if(factor2_s == 0) {
            t = result2 / factor2_t;
            s = (result1 - factor1_t * t)/ factor1_s;
        }else if(factor2_t == 0) {
            s = result2 / factor2_s;
            t = (result1 - factor1_s * s) / factor1_t;
        }else {
            /**
             * factor1_s * s + factor1_t * t = result1
             * factor2_s * s + factor2_t * t = result2
             * 消除factor1_t和factor2_t后，两个式子相减，求s、之后带入任意式子求t
             */
            factor1_s /= factor1_t;
            result1 /= factor1_t;

            factor2_s /= factor2_t;
            result2 /= factor2_t;
            s = (result1 - result2) / (factor1_s - factor2_s);
            t = result2 - factor2_s * s;
        }
        System.out.println("S:" + s + "\t T:" + t);
        Point P, Q;
        double X = x1 + s * (x2 - x1), Y = y1 + s * (y2 - y1), Z = z1 + s * (z2 - z1);
        double U = x3 + t * (x4 - x3), V = y3 + t * (y4 - y3), W = z3 + t * (z4 - z3);
        P = new Point(X, Y, Z);
        Q = new Point(U, V, W);
        if(0 <= s && s <= 1 && 0 <= t && t <= 1){
            return Math.sqrt(Math.pow(P.x - Q.x, 2) + Math.pow(P.y - Q.y, 2) + Math.pow(P.z - Q.z, 2));
        }

        double ACD = SegmentUtils.calculatePointToSegmentDist(A, C, D);
        double BCD = SegmentUtils.calculatePointToSegmentDist(B, C, D);
        double CAB = SegmentUtils.calculatePointToSegmentDist(C, A, B);
        double DAB = SegmentUtils.calculatePointToSegmentDist(D, A, B);
        double min = MathUtils.getMinNum(ACD, BCD, CAB, DAB);
        return min;
    }

    /**
     * 计算点p到线段AB的最小距离 <br/>
     * 思路：
     *  （1）判断p分别到A、B的距离，如果为“0”，则距离为0
     *  （2）判断AB线段的距离，如果为“0”，则距离为pA（pA = pB）
     *  （3）三角形pAB： 如果角ABp为钝角，则最短距离就是pB。 如果角BAp是钝角，则最短距离是pA
     *          否则，以p为顶点的高在AB线段内，使用面积公式计算以AB为底边的高，就是最短距离。
     * @param p：点
     * @param A： 线段AB的顶点A
     * @param B：线段AB的顶点B
     * @return
     */
    public static double calculatePointToSegmentDist(Point p, Point A, Point B) {
        double pA = PointUtils.calculatePointsDist(p, A);
        double pB = PointUtils.calculatePointsDist(p, B);
        double AB = PointUtils.calculatePointsDist(B, A);
        if(MathUtils.isZero(pA) || MathUtils.isZero(pB)) {
            //可认为p就是A或者B
            return 0;
        }
        if(MathUtils.isZero(AB)) {
            //可认为AB就是一个点，此时pA = pB
            return pA;
        }
        if(pA * pA >= pB * pB + AB * AB) {
            //角ABp是钝角,对边是pA，则此时最小距离是pB
            return pB;
        }

        if(pB * pB >= pA * pA + AB * AB) {
            //角BAp是钝角，对边是pB，则此时最小距离是pA
            return pA;
        }

        /*
         *  角BAp < 90度  || 角ABp < 90度，此时p到AB的垂直距离pq，点q在线段AB内；
         *  用海伦公式求三角形pAB的面积，根据三角形面积公式计算pq
         */
        double l = (AB + pA + pB) / 2;
        double area = Math.sqrt(l * (l - pA) * (l - pB) * (l - AB));
        double pq = 2 * area / AB;
        return pq;
    }

    /**
     * 判断线段 AB、CD是否平行
     * @param A
     * @param B
     * @param C
     * @param D
     * @return
     */
    public static boolean isParallel(Point A, Point B, Point C, Point D){
        //求线段的方向向量 AB = {m1, l1, n1};  CD = {m2, l2, n2}
        double m1 = B.x - A.x, l1 = B.y - A.y, n1 = B.z - A.z;
        double m2 = D.x - C.x, l2 = D.y - C.y, n2 = D.z - C.z;
        Vector vector_AB = new Vector(m1, l1, n1);
        Vector vector_CD = new Vector(m2, l2, n2);
        return vector_AB.equals(vector_CD);
    }
}
